using System;
using System.Diagnostics;
using System.Runtime.InteropServices;

namespace RSEngine.Math
{
    /// <summary>
    /// Standard 3-dimensional vector.
    /// </summary>
    /// <remarks>
    ///	A direction in 3D space represented as distances along the 3
    ///	orthoganal axes (x, y, z). Note that positions, directions and
    ///	scaling factors can be represented by a vector, depending on how
    ///	you interpret the values.
    /// </remarks>
    [StructLayout(LayoutKind.Sequential, Pack = 1)]
    public struct Vector3
    {

        #region Variables

        public float x;
        public float y;
        public float z;

        private static readonly Vector3 positiveInfinityVector = new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
        private static readonly Vector3 negativeInfinityVector = new Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
        private static readonly Vector3 invalidVector = new Vector3(float.NaN, float.NaN, float.NaN);
        private static readonly Vector3 zeroVector = new Vector3(0.0f, 0.0f, 0.0f);
        private static readonly Vector3 unitX = new Vector3(1.0f, 0.0f, 0.0f);
        private static readonly Vector3 unitY = new Vector3(0.0f, 1.0f, 0.0f);
        private static readonly Vector3 unitZ = new Vector3(0.0f, 0.0f, 1.0f);
        private static readonly Vector3 negativeUnitX = new Vector3(-1.0f, 0.0f, 0.0f);
        private static readonly Vector3 negativeUnitY = new Vector3(0.0f, -1.0f, 0.0f);
        private static readonly Vector3 negativeUnitZ = new Vector3(0.0f, 0.0f, -1.0f);
        private static readonly Vector3 unitVector = new Vector3(1.0f, 1.0f, 1.0f);

        #endregion

        #region Constructors

        /// <summary>
        ///		Creates a new 3 dimensional Vector.
        /// </summary>
        public Vector3(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        /// <summary>
        ///		Creates a new 3 dimensional Vector.
        /// </summary>
        public Vector3(float unitDimension)
            : this(unitDimension, unitDimension, unitDimension)
        {
        }

        /// <summary>
        ///		Creates a new 3 dimensional Vector.
        /// </summary>
        public Vector3(float[] coordinates)
        {
            if (coordinates.Length != 3)
                throw new ArgumentException("The coordinates array must be of length 3 to specify the x, y, and z coordinates.");
            this.x = coordinates[0];
            this.y = coordinates[1];
            this.z = coordinates[2];
        }

        #endregion

        #region Properties

        public static Vector3 PositiveInfinity
        {
            get
            {
                return positiveInfinityVector;
            }
        }

        public static Vector3 NegativeInfinity
        {
            get
            {
                return negativeInfinityVector;
            }
        }

        public static Vector3 Invalid
        {
            get
            {
                return invalidVector;
            }
        }

        public bool IsZero
        {
            get
            {
                return this.x == 0f && this.y == 0f && this.z == 0f;
            }
        }

        /// <summary>
        /// Returns true if this vector is zero length.
        /// </summary>
        public bool IsZeroLength
        {
            get
            {
                float sqlen = (x * x) + (y * y) + (z * z);
                return (sqlen < (1e-06f * 1e-06f));
            }
        }

        public bool IsInvalid
        {
            get
            {
                return Double.IsNaN(x) || Double.IsNaN(y) || Double.IsNaN(z);
            }
        }

        /// <summary>
        /// Gets the length (magnitude) of this Vector3.  The Sqrt operation is expensive, so 
        /// only use this if you need the exact length of the Vector.  If vector lengths are only going
        /// to be compared, use LengthSquared instead.
        /// </summary>
        public float Length
        {
            get
            {
                return (float)System.Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
            }
        }

        /// <summary>
        ///    Returns the length (magnitude) of the vector squared.
        /// </summary>
        public float LengthSquared
        {
            get
            {
                return (this.x * this.x + this.y * this.y + this.z * this.z);
            }
        }

        #endregion

        #region Operators

        /// <summary>
        ///	User to compare two Vector3 instances for equality.
        /// </summary>
        public static bool operator ==(Vector3 left, Vector3 right)
        {
            return (left.x == right.x && left.y == right.y && left.z == right.z);
        }

        /// <summary>
        ///	User to compare two Vector3 instances for inequality.
        /// </summary>
        public static bool operator !=(Vector3 left, Vector3 right)
        {
            return (left.x != right.x || left.y != right.y || left.z != right.z);
        }

        /// <summary>
        ///	Used when a Vector3 is multiplied by another vector.
        /// </summary>
        public static Vector3 operator *(Vector3 left, Vector3 right)
        {
            return new Vector3(left.x * right.x, left.y * right.y, left.z * right.z);
        }

        /// <summary>
        ///	Used when a Vector3 is divided by another vector.
        /// </summary>
        public static Vector3 operator /(Vector3 left, Vector3 right)
        {
            return new Vector3(left.x / right.x, left.y / right.y, left.z / right.z);
        }

        /// <summary>
        /// Used to divide a vector by a scalar value.
        /// </summary>
        public static Vector3 operator /(Vector3 left, float scalar)
        {
            Debug.Assert(scalar != 0.0f, "Cannot divide a Vector3 by zero.");

            Vector3 vector = new Vector3();

            // get the inverse of the scalar up front to avoid doing multiple divides later
            float inverse = 1.0f / scalar;

            vector.x = left.x * inverse;
            vector.y = left.y * inverse;
            vector.z = left.z * inverse;

            return vector;
        }

        /// <summary>
        ///	Used when a Vector3 is added to another Vector3.
        /// </summary>
        public static Vector3 operator +(Vector3 left, Vector3 right)
        {
            return new Vector3(left.x + right.x, left.y + right.y, left.z + right.z);
        }

        /// <summary>
        ///	Used when a Vector3 is multiplied by a scalar value.
        /// </summary>
        public static Vector3 operator *(Vector3 left, float scalar)
        {
            return new Vector3(left.x * scalar, left.y * scalar, left.z * scalar);
        }

        /// <summary>
        ///	Used when a scalar value is multiplied by a Vector3.
        /// </summary>
        public static Vector3 operator *(float scalar, Vector3 right)
        {
            return new Vector3(right.x * scalar, right.y * scalar, right.z * scalar);
        }

        /// <summary>
        ///	Used to subtract a Vector3 from another Vector3.
        /// </summary>
        public static Vector3 operator -(Vector3 left, Vector3 right)
        {
            return new Vector3(left.x - right.x, left.y - right.y, left.z - right.z);
        }

        /// <summary>
        ///	Used to negate the elements of a vector.
        /// </summary>
        public static Vector3 operator -(Vector3 left)
        {
            return new Vector3(-left.x, -left.y, -left.z);
        }

        /// <summary>
        /// Returns true if the vector's scalar components are all smaller
        /// that the ones of the vector it is compared against.
        /// </summary>
        public static bool operator >(Vector3 left, Vector3 right)
        {
            if (left.x > right.x && left.y > right.y && left.z > right.z)
            {
                return true;
            }

            return false;
        }

        /// <summary>
        /// Returns true if the vector's scalar components are all greater
        /// that the ones of the vector it is compared against.
        /// </summary>
        public static bool operator <(Vector3 left, Vector3 right)
        {
            if (left.x < right.x && left.y < right.y && left.z < right.z)
                return true;

            return false;
        }

        /// <summary>
        /// Convert to a Vector4.
        /// </summary>
        public static explicit operator Vector4(Vector3 vec3)
        {
            return new Vector4(vec3.x, vec3.y, vec3.z, 1.0f);
        }

        /// <summary>
        ///	Used to access a Vector by index 0 = x, 1 = y, 2 = z.  
        /// </summary>
        /// <remarks>
        ///	Uses unsafe pointer arithmetic to reduce the code required.
        ///	</remarks>
        public float this[int index]
        {
            get
            {
                Debug.Assert(index >= 0 && index < 3, "Indexer boundaries overrun in Vector3.");

                // using pointer arithmetic here for less code.  Otherwise, we'd have a big switch statement.
                unsafe
                {
                    fixed (float* pX = &x)
                        return *(pX + index);
                }
            }
            set
            {
                Debug.Assert(index >= 0 && index < 3, "Indexer boundaries overrun in Vector3.");

                // using pointer arithmetic here for less code.  Otherwise, we'd have a big switch statement.
                unsafe
                {
                    fixed (float* pX = &x)
                        *(pX + index) = value;
                }
            }
        }

        #endregion

        #region Public methods

        public object[] ToObjectArray()
        {
            return new object[] { x, y, z };
        }
        public float[] ToArray()
        {
            return new float[] { x, y, z };
        }

        public bool IsAnyComponentGreaterThan(Vector3 vector)
        {
            return (this.x > vector.x || this.y > vector.y || this.z > vector.z);
        }

        public bool IsAnyComponentGreaterThanOrEqualTo(Vector3 vector)
        {
            return (this.x >= vector.x || this.y >= vector.y || this.z >= vector.z);
        }

        public bool IsAnyComponentLessThan(Vector3 vector)
        {
            return (this.x < vector.x || this.y < vector.y || this.z < vector.z);
        }
        public bool IsAnyComponentLessThanOrEqualTo(Vector3 vector)
        {
            return (this.x <= vector.x || this.y <= vector.y || this.z <= vector.z);
        }

        public Vector3 Offset(float x, float y, float z)
        {
            return new Vector3(this.x + x, this.y + y, this.z + z);
        }

        /// <summary>
        ///	Performs a Dot Product operation on 2 vectors, which produces the angle between them.
        /// </summary>
        public float Dot(Vector3 vector)
        {
            return x * vector.x + y * vector.y + z * vector.z;
        }

        /// <summary>
        ///	Performs a Cross Product operation on 2 vectors, which returns a vector that is perpendicular
        ///	to the intersection of the 2 vectors.  Useful for finding face normals.
        /// </summary>
        public Vector3 Cross(Vector3 vector)
        {
            return new Vector3(
                (this.y * vector.z) - (this.z * vector.y),
                (this.z * vector.x) - (this.x * vector.z),
                (this.x * vector.y) - (this.y * vector.x)
                );
        }

        /// <summary>
        /// Returns the angle between theses 2 vectors.
        /// </summary>
        public float Angle(Vector3 vector)
        {
            Vector3 v1 = this.Clone();
            Vector3 v2 = vector.Clone();
            
            v1.Normalize();
            v2.Normalize();

            return (float)System.Math.Acos(v1.Dot(v2));
        }

        /// <summary>
        /// Returns the signed angle between 2 vectors.
        /// A surface normal is provided.
        /// </summary>
        public float SignedAngle3D(Vector3 v2, Vector3 normal)
        {
            float a1 = this.Cross(v2).Dot(normal);
            float a2 = this.Dot(v2);

            return (float)System.Math.Atan2(a1, a2);
        }

        /// <summary>
        ///	Finds a vector perpendicular to this one.
        /// </summary>
        public Vector3 Perpendicular()
        {
            Vector3 result = this.Cross(Vector3.UnitX);

            // check length
            if (result.LengthSquared < float.Epsilon)
            {
                // This vector is the Y axis multiplied by a scalar, so we have to use another axis
                result = this.Cross(Vector3.UnitY);
            }

            return result;
        }
        public static Vector3 SymmetricRandom()
        {
            return SymmetricRandom(1f, 1f, 1f);
        }

        public static Vector3 SymmetricRandom(Vector3 maxComponentMagnitude)
        {
            return SymmetricRandom(maxComponentMagnitude.x, maxComponentMagnitude.y, maxComponentMagnitude.z);
        }

        public static Vector3 SymmetricRandom(float maxComponentMagnitude)
        {
            return SymmetricRandom(maxComponentMagnitude, maxComponentMagnitude, maxComponentMagnitude);
        }

        public static Vector3 SymmetricRandom(float xMult, float yMult, float zMult)
        {
            return new Vector3(
                (xMult == 0) ? 0 : xMult * MathHelper.SymmetricRandom(),
                (yMult == 0) ? 0 : yMult * MathHelper.SymmetricRandom(),
                (zMult == 0) ? 0 : zMult * MathHelper.SymmetricRandom()
                );
        }

        /// <summary>
        /// 
        /// </summary>
        public Vector3 RandomDeviant(float angle, Vector3 up)
        {
            Vector3 newUp = Vector3.Zero;

            if (up == Vector3.Zero)
                newUp = this.Perpendicular();
            else
                newUp = up;

            // rotate up vector by random amount around this
            Quaternion q = Quaternion.FromAngleAxis(this, MathHelper.UnitRandom() * MathHelper.TWO_PI);
            newUp = q * newUp;

            // finally, rotate this by given angle around randomized up vector
            q = Quaternion.FromAngleAxis(newUp, angle);

            return q * this;
        }

        ///<overloads>
        ///<summary>Returns wether this vector is within a positional tolerance of another vector</summary>
        ///<param name="right">The vector to compare with</param>
        ///</overloads>
        ///<remarks>Uses a defalut tolerance of 1E-03</remarks>
        public bool FuzzyEquals(Vector3 right)
        {
            return FuzzyEquals(right, 1e-03f);
        }

        /// <param name="tolerance">The amount that each element of the vector may vary by and still be considered equal.</param>
        public bool FuzzyEquals(Vector3 right, float tolerance)
        {
            return MathHelper.FuzzyEqual(x, right.x, tolerance) &&
                   MathHelper.FuzzyEqual(y, right.y, tolerance) &&
                   MathHelper.FuzzyEqual(z, right.z, tolerance);
        }

        /// <summary>
        ///	Finds the midpoint between the supplied Vector and this vector.
        /// </summary>
        /// <param name="vector"></param>
        /// <returns></returns>
        public Vector3 MidPoint(Vector3 vector)
        {
            return new Vector3((this.x + vector.x) * 0.5f,
                (this.y + vector.y) * 0.5f,
                (this.z + vector.z) * 0.5f);
        }

        /// <summary>
        ///	Compares the supplied vector and updates it's x/y/z components of they are higher in value.
        /// </summary>
        /// <param name="compare"></param>
        public Vector3 Ceil(Vector3 compare)
        {
            Vector3 newVector = new Vector3(this.x, this.y, this.z);
            if (compare.x > x)
                newVector.x = compare.x;
            if (compare.y > y)
                newVector.y = compare.y;
            if (compare.z > z)
                newVector.z = compare.z;

            return newVector;
        }

        /// <summary>
        ///	Compares the supplied vector and updates it's x/y/z components of they are lower in value.
        /// </summary>
        /// <param name="compare"></param>
        /// <returns></returns>
        public Vector3 Floor(Vector3 compare)
        {
            Vector3 newVector = new Vector3(this.x, this.y, this.z);
            if (compare.x < x)
                newVector.x = compare.x;
            if (compare.y < y)
                newVector.y = compare.y;
            if (compare.z < z)
                newVector.z = compare.z;

            return newVector;
        }

        public bool AllComponentsLessThan(float limit)
        {
            return System.Math.Abs(x) < limit && System.Math.Abs(y) < limit && System.Math.Abs(z) < limit;
        }

        public bool DifferenceLessThan(Vector3 other, float limit)
        {
            return System.Math.Abs(x - other.x) < limit && System.Math.Abs(y - other.y) < limit && System.Math.Abs(z - other.z) < limit;
        }

        /// <summary>
        /// Determine if the vector from v1 to v2 and onwards crosses the vector from v3 to v4.
        /// </summary>
        static public bool IsCrossing(Vector3 v1, Vector3 v2, Vector3 v3, Vector3 v4)
        {
            Vector3 dir, e1, e2, p1, p2;
            dir = v2 - v1;
            dir.Normalize();
            e1 = v3 - v2;
            e2 = v4 - v2;
            //check if either of the points are behind
            if (e1.Dot(dir) < 0) return false;
            if (e2.Dot(dir) < 0) return false;
            p1 = e1 - (e1 * dir) * dir;
            p2 = e2 - (e2 * dir) * dir;
            //don't care about positive crossings of v1--v2 and v3--v4, because we won't test these
            //check if the perpendicular vectors are on opposite sides of the direction
            if (p1.Dot(p2) <= 0.0) return true;
            return false;
        }

        /// <summary>
        ///	Gets the shortest arc quaternion to rotate this vector to the destination vector. 
        /// </summary>
        /// <remarks>
        ///	Don't call this if you think the dest vector can be close to the inverse
        ///	of this vector, since then ANY axis of rotation is ok.
        ///	</remarks>
        public Quaternion GetRotationTo(Vector3 destination)
        {
            return GetRotationTo(destination, Vector3.Zero);
        }

        /// <summary>
        /// Gets the shortest arc quaternion to rotate this vector to the destination vector. 
        /// </summary>
        /// <param name="destination"></param>
        /// <param name="fallbackAxis"></param>
        /// <returns></returns>
        /// <remarks>
        ///	Don't call this if you think the dest vector can be close to the inverse
        ///	of this vector, since then ANY axis of rotation is ok.
        ///	</remarks>
        public Quaternion GetRotationTo(Vector3 destination, Vector3 fallbackAxis)
        {
            // Based on Stan Melax's article in Game Programming Gems
            Quaternion q = new Quaternion();

            // Copy, since cannot modify local
            Vector3 v0 = new Vector3(this.x, this.y, this.z);
            Vector3 v1 = destination;

            // normalize both vectors 
            v0.Normalize();
            v1.Normalize();

            // get the cross product of the vectors
            Vector3 c = v0.Cross(v1);

            float d = v0.Dot(v1);

            // If dot == 1, vectors are the same
            if (d >= 1.0f)
            {
                return Quaternion.Identity;
            }

            if (d < (1e-6f - 1.0f))
            {
                if (fallbackAxis != Vector3.Zero)
                {
                    // rotate 180 degrees about the fallback axis
                    q = Quaternion.FromAngleAxis(fallbackAxis, MathHelper.PI);
                }
                else
                {
                    // Generate an axis
                    Vector3 axis = Vector3.UnitX.Cross(this);
                    if (axis.IsZeroLength) // pick another if colinear
                        axis = Vector3.UnitY.Cross(this);
                    axis.Normalize();
                    q = Quaternion.FromAngleAxis(axis, MathHelper.PI);
                }
            }
            else
            {
                float s = (float)System.Math.Sqrt((1 + d) * 2);
                float inverse = 1f / s;

                q.x = c.x * inverse;
                q.y = c.y * inverse;
                q.z = c.z * inverse;
                q.w = s * 0.5f;
            }
            return q;
        }

        public Vector3 ToNormalized()
        {
            Vector3 vec = this;
            vec.Normalize();
            return vec;
        }

        /// <summary>
        ///	Normalizes the vector.
        /// </summary>
        /// <remarks>
        ///	This method normalises the vector such that it's
        ///	length / magnitude is 1. The result is called a unit vector.
        ///	<p/>
        ///	This function will not crash for zero-sized vectors, but there
        ///	will be no changes made to their components.
        ///	</remarks>
        ///	<returns>The previous length of the vector.</returns>
        public float Normalize()
        {
            float length = (float)System.Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);

            // Will also work for zero-sized vectors, but will change nothing
            if (length > float.Epsilon)
            {
                float inverseLength = 1f / length;

                this.x *= inverseLength;
                this.y *= inverseLength;
                this.z *= inverseLength;
            }

            return length;
        }

        /// <summary>
        /// Calculates a reflection vector to the plane with the given normal.
        /// </summary>
        /// <remarks>
        /// Assumes this vector is pointing AWAY from the plane, invert if not.
        /// </remarks>
        /// <param name="normal">Normal vector on which this vector will be reflected.</param>
        /// <returns></returns>
        public Vector3 Reflect(Vector3 normal)
        {
            return this - (2 * this.Dot(normal) * normal);
        }

        public static Vector3 Transmit(Vector3 rayDirection, Vector3 normal, float n1, float n2)
        {
            Vector3 t;
            float w, r, k;

            rayDirection = rayDirection.Clone();
            normal = normal.Clone();
            rayDirection.Normalize();
            normal.Normalize();
            if (normal.Dot(rayDirection) > 0)
            {
                normal = -normal; 
                r = n1;
                n1 = n2; 
                n2 = r;
            }
            r = n1 / n2;
            w = -rayDirection.Dot(normal) * r;
            k = 1f + (w - r) * (w + r);

            // Check for total internal reflection
            if (k < 0.0)
                return -rayDirection.Reflect(normal);

            k = (float)System.Math.Sqrt(k);
            t = r * rayDirection + (w - k) * normal;
            t.Normalize();

            return t;
        }

        public void CopyFrom(Vector3 source)
        {
            this.x = source.x;
            this.y = source.y;
            this.z = source.z;
        }

        public void CopyTo(Vector3 source)
        {
            source.x = this.x;
            source.y = this.y;
            source.z = this.z;
        }

        #endregion

        #region Static Constant Properties

        /// <summary>
        ///		Gets a Vector3 with all components set to 0.
        /// </summary>
        public static Vector3 Zero
        {
            get
            {
                return zeroVector;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with all components set to 1.
        /// </summary>
        public static Vector3 UnitScale
        {
            get
            {
                return unitVector;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the X set to 1, and the others set to 0.
        /// </summary>
        public static Vector3 UnitX
        {
            get
            {
                return unitX;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the Y set to 1, and the others set to 0.
        /// </summary>
        public static Vector3 UnitY
        {
            get
            {
                return unitY;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the Z set to 1, and the others set to 0.
        /// </summary>
        public static Vector3 UnitZ
        {
            get
            {
                return unitZ;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the X set to -1, and the others set to 0.
        /// </summary>
        public static Vector3 NegativeUnitX
        {
            get
            {
                return negativeUnitX;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the Y set to -1, and the others set to 0.
        /// </summary>
        public static Vector3 NegativeUnitY
        {
            get
            {
                return negativeUnitY;
            }
        }

        /// <summary>
        ///		Gets a Vector3 with the Z set to -1, and the others set to 0.
        /// </summary>
        public static Vector3 NegativeUnitZ
        {
            get
            {
                return negativeUnitZ;
            }
        }

        #endregion

        #region Clone

        public Vector3 Clone()
        {
            Vector3 clone = new Vector3();
            clone.x = x;
            clone.y = y;
            clone.z = z;
            return clone;
        }

        #endregion

        #region Misc

        public override string ToString()
        {
            return string.Format("Vector3({0}, {1}, {2})", this.x, this.y, this.z);
        }

        public override int GetHashCode()
        {
            return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
        }

        public override bool Equals(object obj)
        {
            return (obj is Vector3) && (this == (Vector3)obj);
        }

        #endregion

    }
}
